#ifndef THRUST_ESTIMATOR_HPP
#define THRUST_ESTIMATOR_HPP

#include <ros/ros.h>
#include <queue>

class ThrustEstimator
{
private:
  const double rho2_ = 0.998; // do not change
  double thr2acc_;
  double thr2acc_init_;
  double P_{1e6};

  bool begin_flag_{false};

  std::queue<std::pair<double, double>> timed_thrust_;

  ros::Time start_time_{ros::Time(0)};

public:
  void init(double thr2acc)
  {
    thr2acc_ = thr2acc;
    thr2acc_init_ = thr2acc;
    start_time_ = ros::Time::now();
  }

  void setBeginFlag(bool flag) { begin_flag_ = flag; }

  void push(const double &thrust)
  {
    if (!begin_flag_) return;

    double time = (ros::Time::now() - start_time_).toSec();
    timed_thrust_.push(std::make_pair(time, thrust));

    while (timed_thrust_.size() > 20) { // 20 is enough for 200hz
      timed_thrust_.pop();
    }
  }

  // 检查估计出来的thr2acc_是否趋于平稳

  /**
   * @param acc_z   acceleration in z direction of body frame
   * @param acc_des desired acceleration in z direction of world frame
   */
  double getThrust(const double acc_z, const double acc_des)
  {
    double t_now = (ros::Time::now() - start_time_).toSec();

    while (begin_flag_ && timed_thrust_.size() >= 1) {
      if (t_now - timed_thrust_.front().first > 0.060) { // for 25hz
        timed_thrust_.pop();
        continue;
      }

      if (t_now - timed_thrust_.front().first < 0.020) {
        break;
      }

      /***********************************************************/
      /* Recursive least squares algorithm with vanishing memory */
      /***********************************************************/
      double thrust = timed_thrust_.front().second;
      timed_thrust_.pop();

      /***********************************/
      /* Model: est_a(2) = thr1acc_ * thr */
      /***********************************/
      double gamma = 1 / (rho2_ + thrust * P_ * thrust);
      double K = gamma * P_ * thrust;
      thr2acc_ = thr2acc_ + K * (acc_z - thrust * thr2acc_);
      P_ = (1 - K * thrust) * P_ / rho2_;

      break;
    }

    return acc_des / thr2acc_;
  }

  void reset()
  {
    thr2acc_ = thr2acc_init_;
    P_ = 1e6;
  }
};

#endif // THRUST_ESTIMATOR_HPP
